/* ArduTracker source code.

	Copyright (C) 2011  Sami Finnilä

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

/*

 Pin assignment:
 
 1. pin (Rx) to your autopilot's datamodem's tx pin.
 3. pin to an NO (normally open) button (aka. action button) the other leg of which is connected to ground.
 9. pin (PWM) to your pan servo's signal in.
 10. pin (PWM) to your tilt servos's signal in.
 11. pin to the anode of a red led.
 12. pin to the anode of a green led.
 
 That's all.

 SETUP:
 
 1. Determine the servo pulse boundaries for your servos and the pan/tilt-mechanism.
 	You can use the "Sweep" example in the Arduino reference to make this easy.
 	Both pan and tilt should move 180 degrees and be centered.
 2.	Determine and set the baud rate of your data modem.
 3. Now modify the 1st switch() within the main loop so that it succesfully hijacks
 	the UTM GPS coordinates of your UAV.
 	
 You're done!
 
 USAGE:
 
 Power up your Aurduino, the servos and your UAS. Now you should see the green led blinking
 indicating that you're receiving the GPS position of your UAV. Now position your UAV as close
 close to the antenna tracker as possible and press the action button once (this saves the GPS
 coordinates of the tracker). Now take your UAV and move it some 20 meters away from the tracker.
 Manually turn the antenna tracker towards your UAV and press the action button again. Now the
 tracker has calculated its position and orientation and will start to track your UAV.
*/


#include <Servo.h> 
 
Servo PanServo,TiltServo; 

#define pi 3.14159

/************************ SETTINGS ******************************/


//Servo pulse boundaries.
#define panMinPulse 600
#define panMaxPulse 2490  
#define tiltMinPulse 550
#define tiltMaxPulse 2450

#define serialBaudRate 57600

#define panDefaultPos 0.5*pi //Pan default position in radians, should be between 0 and pi.
#define tiltDefaultPos 0.0	//Tilt default position...

#define overlapFactor 1.1 //The factor that determines how much the heading
							//must go over the servo limits (180 degrees) for
							//the tilt to switch to the other side. I.e.
							// 1.1 as default setting gives you an 18 degree "gray
							// area" where the pan/tilt doesn't switch sides even
							//if the pan servo goes to its limit.

#define servoRefreshInterval 20 //Defines the refresh rate and thus the speed of the servos.
								//The lover the value the faster the servos move.


/****************************************************************/

 
int SerialStatus=0;
int trackerStatus=0;

bool singPositive=true;

byte buf[31];
int bufPointer=0;
bool gotNewFix=false;

int greenLed=12;
int redLed=11;
int actionButton=3;

int tiltCurrentPos=(tiltDefaultPos*180/pi),panCurrentPos=(panDefaultPos*180/pi);


long utm_east=0;
long utm_north=0;
long * tempPointer;
long alt=0;
byte CRCa=0,CRCb=0;

unsigned long time=0,time_last=0;

#define UNINIT 0
#define GOT_STX 1
#define GOT_LENGHT 2
#define GOT_PAYLOAD 3
#define GOT_CRCa 4
#define GOT_WHOLE_MESSAGE 5

#define GOT_TRACKER_POS 1
#define GOT_TRACKER_ORIENTATION 2

long tracker_utm_east=0;
long tracker_utm_north=0;
long tracker_alt=0;
float trackerHeading=0;

float trackerPanSetpoint=panDefaultPos;
float trackerTiltSetpoint=tiltDefaultPos;

 
void setup() 
{ 
  PanServo.attach(9,panMinPulse,panMaxPulse);  
  TiltServo.attach(10,tiltMinPulse,tiltMaxPulse);
  PanServo.write(panDefaultPos*180/pi);
  TiltServo.write(tiltDefaultPos*180/pi);
  time=time_last=millis();
  Serial.begin(serialBaudRate);
  pinMode(greenLed,OUTPUT);
  pinMode(redLed,OUTPUT);
  pinMode(actionButton,INPUT);
  digitalWrite(actionButton,HIGH);
  digitalWrite(greenLed,LOW);
  digitalWrite(redLed,LOW); 
} 



 
 
void loop() 
{ 
  
  
  if(Serial.available())
  {
    byte c=Serial.read();
    switch(SerialStatus)
    {
      case UNINIT:
        
        if(c==0x99)
        {
          SerialStatus++;
          bufPointer=0;
          
        }
        break;
        
      case GOT_STX:
        if(c==0x21)
        {
          SerialStatus++;
          
          CRCa=CRCb=0x21;
        }
        else SerialStatus=UNINIT;
        break;
      case GOT_LENGHT:
        buf[bufPointer++]=c;
        
        CRCa+=c;
        CRCb+=CRCa;
        
        if(bufPointer>=29)SerialStatus++;
        break;
      case GOT_PAYLOAD:
        if(c==CRCa)SerialStatus++;
        else SerialStatus=UNINIT;
        break;
      case GOT_CRCa:
        if(c==CRCb)SerialStatus++;
        else SerialStatus=UNINIT;
        break;
      case GOT_WHOLE_MESSAGE:
        if(buf[1]==8)
        {
          
          utm_east=*((long *)&buf[3]);
          utm_north=*((long *)&buf[7]);
          alt=*((long *)&buf[13]);
          gotNewFix=true;
          digitalWrite(greenLed,HIGH);
          
        }
        
        
        SerialStatus=UNINIT;
        break;
      default:
        SerialStatus=UNINIT;
    }
  }  
  
  double deltaEasting=0;
  double deltaNorthing=0;
  double deltaAlt=0;
  
  double distance=0;
  switch(trackerStatus)
  {
    
    
    case UNINIT:
      trackerPanSetpoint=panDefaultPos;
      trackerTiltSetpoint=tiltDefaultPos;
      
      if(digitalRead(actionButton)==LOW)
      {
        digitalWrite(redLed,HIGH);
        while(digitalRead(actionButton)==LOW);
        digitalWrite(redLed,LOW);
        tracker_utm_east=utm_east;
        tracker_utm_north=utm_north;
        tracker_alt=alt;
        trackerStatus++;
      }
      break;
    case GOT_TRACKER_POS:
      if(digitalRead(actionButton)==LOW)
      {
        digitalWrite(redLed,HIGH);
        while(digitalRead(actionButton)==LOW);
        digitalWrite(redLed,LOW);
        deltaEasting=utm_east-tracker_utm_east;
        deltaNorthing=utm_north-tracker_utm_north;
        
        if(deltaEasting==0) //Easting cannot be zero if we want to calculate the atan(northing/easting).
        {
          if(deltaNorthing<0)trackerHeading=-(pi)/2;
          else if(deltaNorthing!=0)trackerHeading=pi/2;
          else break;
          trackerStatus++;
          break;
        }
        if(deltaNorthing==0)
        {
          if(deltaEasting<0)trackerHeading=pi;
          else trackerHeading=0;
          trackerStatus++;
          break;
        }
        
        trackerHeading=atan(deltaNorthing/deltaEasting);
        
        if(deltaNorthing<0)if(trackerHeading>0)trackerHeading-=pi;
        if(deltaNorthing>0)if(trackerHeading<0)trackerHeading+=pi;
        
        trackerStatus++;
        break;
      }
      break;
    case GOT_TRACKER_ORIENTATION: //Now we can do the tracking...
      
      if(!gotNewFix)break;
      gotNewFix=false;
      
      
      deltaEasting=utm_east-tracker_utm_east;
      deltaNorthing=utm_north-tracker_utm_north;
      deltaAlt=alt-tracker_alt;
      if(deltaAlt<0)deltaAlt=0;
      
      distance=sqrt((deltaEasting*deltaEasting)+(deltaNorthing*deltaNorthing));
      
      if(distance>1000)trackerTiltSetpoint=atan(deltaAlt/distance);
      else 
      {
        trackerTiltSetpoint=pi/2;
        break;
      }
      
      if(deltaEasting==0) //Easting cannot be zero if we want to calculate the atan(northing/easting).
      {
        if(deltaNorthing<0)trackerPanSetpoint=-(pi)/2;
        else if(deltaNorthing!=0)trackerPanSetpoint=pi/2;
        
      }
      else if(deltaNorthing==0)
      {
        if(deltaEasting<0)trackerPanSetpoint=pi;
        else trackerPanSetpoint=0;
      }
      else
      {
        trackerPanSetpoint=atan(deltaNorthing/deltaEasting);
      
        if(deltaNorthing<0)if(trackerPanSetpoint>0)trackerPanSetpoint-=pi;
        if(deltaNorthing>0)if(trackerPanSetpoint<0)trackerPanSetpoint+=pi;
      
        trackerPanSetpoint-=trackerHeading;
      }
      
      trackerPanSetpoint+=panDefaultPos;
      
      if(trackerPanSetpoint<= (overlapFactor * (-pi)))trackerPanSetpoint+=2*pi;
      else if(trackerPanSetpoint>(overlapFactor * pi))trackerPanSetpoint-=2*pi;
      
      if(trackerPanSetpoint<= (overlapFactor * (-pi)))trackerPanSetpoint+=2*pi;
      else if(trackerPanSetpoint>(overlapFactor * pi))trackerPanSetpoint-=2*pi;
      
      
      break;
    default:
      trackerStatus=UNINIT;
  }
  
  time=millis();
  if((time-time_last)>=servoRefreshInterval)
  {
    time_last=time;
    
    digitalWrite(greenLed,LOW);
    
    int panCommand=(int)(180*(trackerPanSetpoint/pi));
    int tiltCommand=(int)180*(trackerTiltSetpoint/pi);
    
    if((trackerPanSetpoint<((overlapFactor-1)*(-pi))) || ((trackerPanSetpoint <= ((overlapFactor-1)*pi)) && !singPositive) )
    {
      tiltCommand=180-tiltCommand;
      panCommand+=180;
    }
    if(trackerPanSetpoint<(overlapFactor-1)*(-pi))singPositive=false;
    else if(trackerPanSetpoint > (overlapFactor-1)*pi)singPositive=true;
    
    
    
    
    if(tiltCurrentPos>tiltCommand)tiltCurrentPos--;
    else if(tiltCurrentPos<tiltCommand)tiltCurrentPos++;
    
    if(panCurrentPos>panCommand)panCurrentPos--;
    else if(panCurrentPos<panCommand)panCurrentPos++;
    
    panCurrentPos=constrain(panCurrentPos,0,180);
    tiltCurrentPos=constrain(tiltCurrentPos,0,180);
  }
  
  PanServo.write(panCurrentPos);
  TiltServo.write(tiltCurrentPos);

} 
